#include <stdio.h>
#include <string.h>
#include "datahandler.h"
#include "example_service_envelope.h"
#include "protocal.h"
#include "usart.h"

//上电从flash中读取均值次数
void Flash_GetAvgForReboot(void)
{
	uint32_t data[64];

	TS_FLASH_MODE_DATA *ts_FlashData = (TS_FLASH_MODE_DATA*)data;

	STMFLASH_Read(FLASH_ADD_PRMDATA, (uint32_t*)data, 64);
    if((ts_FlashData->modbusReg.band<=7)
       && (ts_FlashData->modbusReg.scanLength<=40500)
           && (ts_FlashData->modbusReg.filterCnt>=1)
               && (ts_FlashData->modbusReg.filterCnt<=20)
                   && (ts_FlashData->modbusReg.scanStart>=150)
                       && (CheckSum((uint8_t*)&ts_FlashData->modbusReg, sizeof(TS_MODBUS_REG))==ts_FlashData->checkSum))
	{
		memcpy(&ts_modbusReg, ts_FlashData, sizeof(TS_MODBUS_REG));
	}
	else
	{
		memcpy(&ts_FlashData->modbusReg, &ts_modbusReg, sizeof(TS_MODBUS_REG));
        ts_FlashData->checkSum = CheckSum((uint8_t*)&ts_FlashData->modbusReg, sizeof(TS_MODBUS_REG));
		STMFLASH_Write(FLASH_ADD_PRMDATA, (uint64_t*)data, 32);
	}

    if(ts_modbusReg.zeroCnt == 0)
    {
    	ts_modbusReg.zeroCnt = 3;
    }

#if (FACT_CHOOSE == C7_V2)
	switch(ts_FlashData->modbusReg.band)
	{
		case 0:
			Usart2_Init(1200);
			break;
		case 1:
			Usart2_Init(2400);
			break;
		case 2:
			Usart2_Init(4800);
			break;
		case 3:
			Usart2_Init(9600);
			break;
		case 4:
			Usart2_Init(19200);
			break;
		case 5:
			Usart2_Init(38400);
			break;
		case 6:
			Usart2_Init(57600);
			break;
		case 7:
			Usart2_Init(115200);
			break;
	}

#elif (FACT_CHOOSE == WANWAN)
    switch(ts_FlashData->modbusReg.band)
	{
		case 0:
			Usart1_Init(1200);
			break;
		case 1:
			Usart1_Init(2400);
			break;
		case 2:
			Usart1_Init(4800);
			break;
		case 3:
			Usart1_Init(9600);
			break;
		case 4:
			Usart1_Init(19200);
			break;
		case 5:
			Usart1_Init(38400);
			break;
		case 6:
			Usart1_Init(57600);
			break;
		case 7:
			Usart1_Init(115200);
			break;
	}
#endif
}

void Flash_StoryPram(void)
{
	uint32_t data[64];
	TS_FLASH_MODE_DATA *ts_FlashData = (TS_FLASH_MODE_DATA*)data;

    memcpy(&ts_FlashData->modbusReg, &ts_modbusReg, sizeof(TS_MODBUS_REG));
    ts_FlashData->checkSum = CheckSum((uint8_t*)&ts_FlashData->modbusReg, sizeof(TS_MODBUS_REG));
	STMFLASH_Write(FLASH_ADD_PRMDATA, (uint64_t*)data, 32);
}

#define MEDIAN_CNT          20
static uint16_t s_distanceBuf[MEDIAN_CNT] = {0};
static uint32_t s_distanceCnt = 0;
void Radar_SendDistance(TS_RADAR_SCAN_RESULT *ts_RadarResult)
{
	uint8_t i = 0;
	uint32_t sumDistance = 0;
	uint16_t iAvgDistance = 0;
	uint32_t iStart = 0;
	uint8_t iAvgCnt = 0;

	if(ts_RadarResult->data > 0)
	{
		if(ts_RadarResult->distance <= SCAN_LEN_MAX)
		{
			iAvgCnt = Get_AvgValue();

            s_distanceBuf[s_distanceCnt++%MEDIAN_CNT] = ts_RadarResult->distance;

            if(s_distanceCnt < iAvgCnt)
            {
            	for(i = 0; i < s_distanceCnt; i++)
            	{
            		sumDistance += s_distanceBuf[i];
            	}
            	iAvgDistance = sumDistance/s_distanceCnt;
            }
            else
            {
				iStart = s_distanceCnt - iAvgCnt;
            	for(i = 0; i < iAvgCnt; i++)
            	{
            		sumDistance += s_distanceBuf[(i+iStart)%AVG_CNT_MAX];
            	}
            	iAvgDistance = sumDistance/iAvgCnt;
            }
		}
	}

	Protocal_SendDistance(iAvgDistance);
}

void Radar_SendBlindDistance(uint16_t distance)
{
	Protocal_SendDistance(distance);
}

void Radar_ResultSort(TS_RADAR_SCAN_RESULT *result, uint8_t cnt)
{
	uint8_t i, j;
	TS_RADAR_SCAN_RESULT tempResult = {0};

	for(i = 0; i < cnt; i++)
	{
		for(j = 0; j < cnt-i-1; j++)
		{
			if(result[j].data < result[j+1].data)
			{
				tempResult.data = result[j].data;
				tempResult.distance = result[j].distance;

				result[j].data = result[j+1].data;
				result[j].distance = result[j+1].distance;

				result[j+1].data = tempResult.data;
				result[j+1].distance = tempResult.distance;
			}
		}
	}
}

uint16_t get_start_scan_index(uint16_t ds0, uint16_t ds1, uint16_t ds2)
{
	if(((ds0>=ds1)&&(ds0<=ds2))	|| ((ds0>=ds2)&&(ds0<=ds1)))
	{
		return 0;
	}
	else if(((ds1>=ds0)&&(ds1<=ds2)) || ((ds1>=ds2)&&(ds1<=ds0)))
	{
		return 1;
	}
	else if(((ds2>=ds0)&&(ds2<=ds1)) || ((ds2>=ds1)&&(ds2<=ds0)))
	{
		return 2;
	}
}

extern uint8_t dist_detecter(void);
extern uint8_t g_radioErrFlag;
extern TS_RADAR_SCAN_RESULT ts_radarResult[RADAR_RESULT_MAX];
extern uint8_t g_radarResCnt;
/*RADAR数据处理，获取距离值*/
void Radar_DataHandler(void)
{
	static uint16_t blind_area_cnt = 0;
	static uint16_t start_scan_cnt = 0;
	static TS_RADAR_SCAN_RESULT ts_start_scan_result[3][RADAR_RESULT_MAX] = {0};

	if(true == dist_detecter())
	{
		g_radioErrFlag = 0;

		if(g_radarResCnt == 0)
		{
			if(blind_area_cnt >= ts_modbusReg.zeroCnt)
			{
				Radar_SendBlindDistance(ts_modbusReg.scanStart);
			}
			else
			{
				blind_area_cnt++;
			}
		}
		else
		{
			blind_area_cnt = 0;
			Radar_ResultSort(ts_radarResult, g_radarResCnt);

			memcpy(&ts_start_scan_result[start_scan_cnt][0], &ts_radarResult[0], sizeof(TS_RADAR_SCAN_RESULT)*RADAR_RESULT_MAX);
			if(++start_scan_cnt >= 3)
			{
				start_scan_cnt = 0;
				uint16_t index = get_start_scan_index(ts_start_scan_result[0][0].distance, ts_start_scan_result[1][0].distance, ts_start_scan_result[2][0].distance);
				Radar_SendDistance(ts_start_scan_result[index]);
				setMutiResult(ts_start_scan_result[index]);
			}
		}

		memset(ts_radarResult, 0, sizeof(ts_radarResult));
		g_radarResCnt = 0;
	}
	else
	{
		g_radioErrFlag = 1;
	}
}

